﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace PayloadTracker.APRS.SoundModem
{

    public delegate void FSKBitReadyDelegate(bool bit);
    /// <summary>
    /// Frequency Shift Keying code based off of 
    /// Robert Krten 's implementation http://fisheye.freeswitch.org/browse/OpenZAP/trunk/src/fsk.c?r=400 and 
    /// article http://blog.chinaunix.net/u/22778/showart_336597.html
    /// </summary>
    public class FSK
    {


        public struct ModemDeffinition
        {
            public int FreqSpace;
            public int FreqMark;
            public int BaudRate;
            public int SampleRate;

            public ModemDeffinition(int FreqSpace, int FreqMark, int BaudRate, int SampleRate)
            {
                this.FreqSpace = FreqSpace;
                this.FreqMark = FreqMark;
                this.BaudRate = BaudRate;
                this.SampleRate = SampleRate;
            }
        }

        public struct CaptureDevice
        {
            public uint ID;
            public string Name;
            public override string ToString()
            {
                return ID.ToString() +": " + Name;
            }
            public CaptureDevice(uint ID, string Name)
            {
                this.ID = ID;
                this.Name = Name;
            }
        }
        public static List<CaptureDevice> GetAvailableDevices()
        {
            var ret = new List<CaptureDevice>();

            int devs = WaveLib.WaveNative.waveInGetNumDevs();

            for(uint i=0;i<=devs;i++)
            {
                WaveLib.WaveNative.WaveInCaps caps = new WaveLib.WaveNative.WaveInCaps();
                WaveLib.WaveNative.waveInGetDevCaps(i, caps, System.Runtime.InteropServices.Marshal.SizeOf(caps));

                CaptureDevice cd = new CaptureDevice();
                cd.Name = caps.szName;
                cd.ID = i;
                
                ret.Add(cd);
            }

            return ret;
        }


        public static readonly ModemDeffinition Bell202 = new ModemDeffinition(2200, 1200, 1200, 44100);//22050
        public event FSKBitReadyDelegate FSKBitReady;

        ModemDeffinition m_Modem;

        WaveLib.WaveInRecorder m_Recorder;
        int m_Device;
        byte[] m_RecordBuffer;

        int m_StateConsecutive;

        double[,] m_Correlates;
        int m_CorrelatesCount;

        double[] m_RingBuffer;
        int m_RingBufferIndex;

        double m_CellPosition;
        double m_CellAdjustment;

        bool m_CurrentBit, m_PreviousBit, m_PreviousProcessedBit;

        int m_DownsamplingCount, m_DownsamplingCurrent;
        int m_QuietConsecutive;

        Filters.LowPass m_LowPass;
        Filters.HighPass m_HighPass;


        public FSK(ModemDeffinition modem, int CaptureDevice)
        {
            Initialize(modem, CaptureDevice, false);
        }
        ~FSK()
        {
            if (m_Recorder != null)
                m_Recorder.Dispose();
        }
        private void Initialize(ModemDeffinition modem, int DeviceID, bool downsample)
        {
            //setup filters
            m_LowPass = new Filters.LowPass(Math.Max(modem.FreqMark, modem.FreqSpace),modem.SampleRate);
            m_HighPass = new Filters.HighPass(Math.Min(modem.FreqMark, modem.FreqSpace), modem.SampleRate);
            m_Modem = modem;
            m_Device = DeviceID;

            m_StateConsecutive = 0;
            m_PreviousProcessedBit = m_CurrentBit = m_PreviousBit = true;

            //see if we can downsample. We only need 6 to match.
            if (downsample && m_Modem.SampleRate / m_Modem.FreqMark > 6)
                m_DownsamplingCount = m_Modem.SampleRate / m_Modem.FreqMark / 6;
            else
                m_DownsamplingCount = 1;

            m_DownsamplingCurrent = 1;


            //Calculate correlate size (aka the number of samples needed for the smallest wave)
            m_CorrelatesCount = m_Modem.SampleRate / m_DownsamplingCount / m_Modem.FreqMark;
            m_Correlates = new double[4, m_CorrelatesCount];

            //initialize them.
            double phi_mark = 2.0f * Math.PI / (((double)m_Modem.SampleRate) / ((double)m_DownsamplingCount) / ((double)m_Modem.FreqMark));
            double phi_space = 2.0f * Math.PI / (((double)m_Modem.SampleRate) / ((double)m_DownsamplingCount) / ((double)m_Modem.FreqSpace));


            for (int i = 0; i < m_CorrelatesCount; i++)
            {
                m_Correlates[0, i] = Math.Sin(phi_mark * ((double)i));
                m_Correlates[1, i] = Math.Cos(phi_mark * ((double)i));
                m_Correlates[2, i] = Math.Sin(phi_space * ((double)i));
                m_Correlates[3, i] = Math.Cos(phi_space * ((double)i));
            }

            //Create the ring buffer
            m_RingBuffer = new double[m_CorrelatesCount];
            for (int i = 0; i < m_RingBuffer.Length; i++)
                m_RingBuffer[i] = 0;

            m_RingBufferIndex = 0;

            m_CellPosition = 0;
            m_CellAdjustment = m_Modem.BaudRate / ((double)m_Modem.SampleRate) * ((double)m_DownsamplingCount);

            m_QuietConsecutive = 10;
        }
        public void Start()
        {
            m_Recorder = new WaveLib.WaveInRecorder(m_Device, new WaveLib.WaveFormat(m_Modem.SampleRate, 16, 1), m_Modem.SampleRate, 3, new WaveLib.BufferDoneEventHandler(OnRecordedData));
        }
        public void Stop()
        {
            if(m_Recorder != null)
                m_Recorder.Dispose();
            m_Recorder = null;
        }
        void OnRecordedData(IntPtr data, int size)
        {
            if(m_RecordBuffer == null || size > m_RecordBuffer.Length)
                m_RecordBuffer = new byte[size];
            
            System.Runtime.InteropServices.Marshal.Copy(data, m_RecordBuffer, 0, size);
            var ms = new System.IO.MemoryStream(m_RecordBuffer);
            var br = new System.IO.BinaryReader(ms);

            for (int i = 0; i < size / 2; i++)
            {
                var sample = br.ReadInt16();
                ProcessSample(sample / 32768.0f);
            }
        }

        public void ProcessSample(double sample)
        {
   
            //pass it through the filters
            //sample = m_LowPass.Process(sample);
            //sample = m_HighPass.Process(sample);

            //see if we can downsample.
            if (m_DownsamplingCount != 1)
            {
                if (m_DownsamplingCurrent < m_DownsamplingCount)
                {
                    m_DownsamplingCurrent++;
                    return; //discard sample.
                }
                else
                    m_DownsamplingCurrent = 1;
            }


            //how loud is this sample?
            if (Math.Abs(sample) < 0.05)
            {
                if (m_QuietConsecutive == 9)
                {//we hit quiet time to reset.
                    //Logger.LOG.WriteLine("Quiet period hit. Resetting FSK.");
                    m_CellPosition = 0;
                    m_StateConsecutive = m_DownsamplingCurrent = 0;
                    m_PreviousProcessedBit = m_CurrentBit = m_PreviousBit = true;
                    m_LowPass.Reset();
                    m_HighPass.Reset();
                    for (int i = 0; i < m_RingBuffer.Length; i++)
                        m_RingBuffer[i] = 0;

                    m_QuietConsecutive = 10;
                    return;
                }
                else if (m_QuietConsecutive == 10)
                    //we are in a quiet period and waiting for a trigger. do nothing.
                    return;
                else
                    m_QuietConsecutive++;

            }
            else 
                m_QuietConsecutive = 0;

                

            Logger.LOG.AudioSample(sample);


            //store sample in ring buffer.
            m_RingBuffer[m_RingBufferIndex++] = sample;
            if (m_RingBufferIndex >= m_RingBuffer.Length)
                m_RingBufferIndex = 0;


            //do correlate calculations 
            double[] factors = { 0, 0, 0, 0 };
            for (int i = 0, j = m_RingBufferIndex; i < m_RingBuffer.Length; i++)
            {
                //handle ring buffer.
                if (j >= m_RingBuffer.Length)
                    j = 0;

                var v = m_RingBuffer[j];

                factors[0] += m_Correlates[0, i] * v;
                factors[1] += m_Correlates[1, i] * v;
                factors[2] += m_Correlates[2, i] * v;
                factors[3] += m_Correlates[3, i] * v;
                j++;
            }

            m_PreviousBit = m_CurrentBit;


            double positive = factors[0] * factors[0] + factors[1] * factors[1];
            double negative = factors[2] * factors[2] + factors[3] * factors[3];

            m_CurrentBit = (positive > negative);

            //if there is a transition we can synchronize the bit cell position

            if (m_PreviousBit != m_CurrentBit)
                //adjust to the middle of the cell.
                m_CellPosition = 0.5;

            m_CellPosition += m_CellAdjustment;

            if (m_CellPosition > 1.0)
            {
                m_CellPosition -= 1.0;

                //AX.25 SPECIFIC CODE 
                //Perform NRZI (Non return to zero inverted) encoding.
                //This means that a 0 bit is encoded as a change in tone while a 1 is encoded as NO CHANGE in tone.
                //If this is not needed do not do the following block of code and just call ProcessBit(m_CurrentBit)
                ProcessBit(m_CurrentBit == m_PreviousProcessedBit);
                m_PreviousProcessedBit = m_CurrentBit;

                //ProcessBit(m_CurrentBit);

            }

        }
        void ProcessBit(bool bit)
        {
            //Logger.LOG.Write(bit?"1":"0");
            FSKBitReadyDelegate handler = FSKBitReady;
            if(handler != null)
                handler(bit);
        }
    }
}
